#!/usr/bin/env python
# -*- encoding: utf-8 -*-
'''
@Copyright    :   Jihua Lab 2021.
@File    :   robot_control_node.py
@Author  :   WANG Hao 
@Contact :   wanghao@jihualab.com、haowanghk@gmail.com(Github)
@License :   (C)Copyright 2020-2021, JHL
@Version :   2.3.4
@Create Time    :  2021-12-20
@Modify Time      @Author    @Version    @Desciption
-------------------        --- -------    ---  --------   --- -----------
@File Description :   DUAL_ARM ros robot control metapackage  
@Update: 2021-12-23 add dual_arm_control threadding and nodes, pre for testing
         2022-2-28 modified the  dual_arm_control threadding and nodes paramters
         2022-3-02 modified the ping host ip
'''

# the import lib
import logging
import threading
import subprocess
import re


import uservo
import serial
import time

# import robot motion control pkg
from MotionGroup import MotionGroup
from ServoPose import Pose
from wrist_gripper import WristGripperControl
from robot_arm import ArmMotionControl

# inport ros py
import rospy  
from sensor_msgs.msg import JointState
from joint_state_exchange.msg import RobotState
from dual_arm_base_control.msg import TelemetryInfo
from dual_arm_base_control.srv import CtrlModeSelect
from std_msgs.msg import String


# flag
exitFlag = 0  # threading control flag
VERBOSE = False #rospr loginfo flag
G_CONTROL_MODE = 3  # control mode server 0,1,2,3 default is landing mode
# Global const para
THREAD_MODE_CONTROL = ["control", "Control", "CONTROL"]  # TH1
THREAD_MODE_PING = ["ping", "ping_ip", "host_ip"]        # TH2
THREAD_MODE_PUB = ["publish", "pub", "publisher"]        # TH3

HOST_STATE = True # the homer host ip port state , default is true

SHORT_TIME_DELAY = 0.02  # time delay para
MEDIA_TIME_DELAY = 0.10
LONG_TIME_DELAY = 1.0

controlPeriod = 1.0

# publisher
SAMPLING_FREQUENCY = 10  # units: Hz
fileName = "robot_arm_records.txt"
POSE_NUM = rospy.get_param('/teach_mode/way_point_num')  # default pose number is 300

# PARAMS
g_ending_servo_idx = [0, 1, 2, 3]
Pi =3.1415926535      
Pi_Para = 57.29577951471995   # Pi_Para = 180.0/Pi


VEL = {'Very_low_spped': 10, 'LOW_SPEED': 15, 'MID_SPEED': 45, 'HIGH_SPEED': 60}

# @ brief:抓取动作初始位置，相对零位姿
#         手臂竖直状态,绝对角度控制模式
#: 双臂机器人起飞及降落状态时手臂必须处于预备蜷缩位置姿态 DUAL_ARM_INIT_POSE
DUAL_ARM_INIT_POSE = [140.0, 50.0, 72.0, 0, 
                        -2.0, -80, 60.0, 0, 
                        40, 40]
# 伺服模式下的零点位置，即手臂竖直向下（小臂关节角度为0）状态为0位，手腕90°、手爪10°
DUAL_ARM_ZERO_ANGLE_POSE = [140.0, 50.0, -30.0, 90,
                          -2.0, -80, -35, 90, 
                          10, 10]
# 抓取目标位置
DUAL_ARM_TARGET_POSE = [140.0, 50.0, -30.0, 0,
                        2.0, -82, -35, 0, 
                        80, 90]

# 计算关节角度绝对角度数值与关节角度相对于零点位置的相对转动角度
dual_arm_joint_angle_controller = []
dual_arm_joint_angle_relatively = []
# dual_arm_joint_angle_relatively = dual_arm_joint_angle_controller - DUAL_ARM_START_POSE

# dual_arm class obj init
dual_arm_ = ArmMotionControl(arm_name="dual_arm", 
                            dof=rospy.get_param('/arm_info/dual_arm_dof'), 
                            CONTROL_MODE="DAMPING", 
                            with_wrist=False, 
                            is_debug=True,
                            damping_power=1000)
wrist_gripper_ = WristGripperControl(servo_channel=g_ending_servo_idx, is_debug=True)

# 初始化舵机管理器类对象、串口端口地址
rospy.loginfo("----DUAL ARM STATE INIT !-----")
dual_arm_obj_ = dual_arm_.creat_uservo_obj(SERVO_BAUDRATE=115200)
if dual_arm_.servo_buffer(is_scan_servo=True):
    # dual arm and gripper init
    dual_arm_.arm_state_init(force_angle_init=False)
    dual_arm_.set_all_servo_mode(CONTROL_MODE="CIRCLE")
    wrist_gripper_.all_ending_init(motion_group=4)
    dual_arm_.logging_arm_info()         # logging servo info 
    dual_arm_.get_arm_current_angle()    # logging servo angle info
    rospy.loginfo("----DUAL ARM STATE INIT: control obj init success!-----")
    rospy.loginfo("----Dual Arm and gripper init success!! --- ")
    time.sleep(1)
else:
    rospy.logerr("ERROR: dual arm and gripper init failed!!!")

# global control obj from dual_control_manager
g_dual_arm_nh = dual_arm_ 
g_wrist_gripper = wrist_gripper_
g_all_servo_list = g_dual_arm_nh.get_arm_servo_list()
# END of Global Var

##### take off and landing dual_arm pose init ####
def pose_init(dual_arm_node_handler,
                wrist_gripper_node_handler,
                servo_list):
    # servo index input check
    if(servo_list == [] or dual_arm_node_handler == []):
        rospy.logerr("ERROR INPUT PARAMTERS: servo_list {} ".format(servo_list))
        return False
    # set robot arms、wrist and grippers into landing pose
    for SERVO_ID in servo_list:
        dual_arm_node_handler.set_single_servo_pose(
            Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID], 
                                    vel=VEL.get('LOW_SPEED'), 
                                    iterval=2000, 
                                    t_acc=40,
                                    t_dec=40, 
                                    power=9000, 
                                    mean_dps=40, 
                                    servo_id=SERVO_ID))
        # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
        wrist_gripper_node_handler.set_servo_angle(servo_idx=0, angle=DUAL_ARM_INIT_POSE[3])
        wrist_gripper_node_handler.set_servo_angle(servo_idx=1, angle=DUAL_ARM_INIT_POSE[8])

        # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
        wrist_gripper_node_handler.set_servo_angle(servo_idx=2, angle=DUAL_ARM_INIT_POSE[7])
        wrist_gripper_node_handler.set_servo_angle(servo_idx=3, angle=DUAL_ARM_INIT_POSE[9])
        time.sleep(SHORT_TIME_DELAY)               
    return True

def rad2deg(rad_val):
    # 弧度制转角度值
    deg = (rad_val * Pi_Para)
    return deg 

#dual arm fly robot 3 threading: control and publisher threads 
# control thread is for main control node , and the pub is the publisher of robot arm states
class dfrThread(threading.Thread):

    def __init__(self, threadID, name, controlPeriod, samplingFrequency):
        threading.Thread.__init__(self)
        self.threadID = threadID
        self.threadName = name                      # thread class object ->name 
        self.controlPeriod = controlPeriod          # control period
        self.samplingFrequency = samplingFrequency  # samplingFrequency
        # self.dual_arm_nh = dual_arm_nh              # dual_arm_node_handler
        # self.wrist_gripper = wrist_gripper          # wrist_gripper control node handler
        self.this_ip = rospy.get_param('/general/this_ip')
        self.host_ip = rospy.get_param('/general/host_ip')   # string type host ip address    

    def run(self):
        rospy.loginfo("THREADING - 开始线程：" + self.threadName)
        if (self.threadName in THREAD_MODE_CONTROL):
            rospy.loginfo("%s: %s" % (self.threadName, time.ctime(time.time())))
			# 初始化ROS Node
         
            if self.check_host_state() == False: 
                # ping false and init pose then
                rospy.loginfo("--Ping Host Ip return False, Please Check! Turn to init pose!  ")
                self.threadName.exit() 
            else:
                rospy.loginfo("--Ping Host Ip return True! thread_of_arm_control() start!")
                self.thread_of_arm_control()           #robot arm control thread

        elif self.threadName in THREAD_MODE_PING:
            rospy.loginfo("%s: %s" % (self.threadName, time.ctime(time.time())))
            # call ping host ip function, and return HOST_STATE true or false
            ping_host_ip(self.threadName,
                        self.this_ip,
                        self.host_ip)

        elif self.threadName in THREAD_MODE_PUB:
            rospy.loginfo("%s: %s" % (self.threadName, time.ctime(time.time())))
            try:
                statePub(rospy.get_param('/node_info/state_pub_freq'))
            except KeyboardInterrupt:
	            rospy.loginfo('Robot Telemetry Info END od Publishing')
        else:
            rospy.logerr_once("self.threadName should be in THREAD_MODE_PUB\PING or THREAD_MODE_CONTROL !")

        # TODO: force to change robot arm state into take off and landing states
        # threading exit -> dual_arm pre to landing 
        rospy.logerr("-----dual_arm_manager exit()")
        rospy.loginfo("----退出线程：" + self.name + '\n')


    ####### thread 1 control- callback #####
    def thread_of_arm_control(self):
        # main function of thread 1 -> dual_arm_control callback
        # construct new control threading
        # node name:    dual_arm_control_node
        # node topic:   NONE

        robot_arm_control_nh = robotArmControl(self.threadName,
                                self.controlPeriod,
                                self.samplingFrequency)
        # robotArmControl class object node handler for control mode select and motion control function
        control_srv = robot_arm_control_nh.robot_arm_control()        # todo srv and mode selct, one time only
        if G_CONTROL_MODE == 0:
            # change into Auto mode
            robot_arm_control_nh.start_auto_mode()
            if not self.check_host_state(): self.exit_thread()
            
        elif G_CONTROL_MODE == 1:
            robot_arm_control_nh.start_teach_mode()
            if not self.check_host_state(): self.exit_thread()

        elif G_CONTROL_MODE == 2:
            robot_arm_control_nh.start_servo_mode()
            if not self.check_host_state(): self.exit_thread()

        elif G_CONTROL_MODE == 3:
            robot_arm_control_nh.start_landing_mode()
            if not self.check_host_state(): self.exit_thread()

        else:
            rospy.logerr("G_CONTROL_MODE ERROR from threading {}".format(self.threadName))
      

  # threading function 
    def exit_thread(self):
        # check host ip address state, emergency exit this node thread
        # ping false and init pose then
        try:
            state_flag = pose_init(dual_arm_node_handler = g_dual_arm_nh,
                wrist_gripper_node_handler = g_wrist_gripper,
                servo_list = g_all_servo_list)
        except KeyboardInterrupt:
            rospy.loginfo('-----Robot Arm control Node ALL END----')
        if state_flag:
            rospy.loginfo("----THREADING - Checking host ip address state error," +
                "emergency exit this node thread : {}".format(self.threadName))
        else:
            rospy.logerr("-----ERROR form : init_flag =  pose_init()")
        # END   
    
    ###########
    # check host state
    # get the HOST_STATE = False or True
    ############    
    def check_host_state(self):
        # check the target host ip state 
        # return Boolean type True or False
        if HOST_STATE :
            return True
        else:
            return False

    def get_host_ip(self):
        # member function  : get target host machine ip address
        return self.host_ip

    def get_this_ip(self):
        # member function : get this machine ip address
        return self.this_ip

###### define the robtoArmControl Class ######
class robotArmControl(object):
    # Obj API 
    def __init__(self,thread_name, control_period, sampling_frequency):
        self.thread_name = thread_name
        # self.dual_arm_nh = dual_arm_nh
        # self.wrist_gripper = wrist_gripper
        self.control_period = control_period
        self.sampling_frequency = sampling_frequency
        # get the robot arm joint servo id list
        # self.servo_list = dual_arm_nh.get_arm_servo_list()
        self.is_debug = False

    # 1. the robot control main thread
    def robot_arm_control(self):
        # dual arm motion control threading,sub  robot and gripper pub
        # to change wrist and gripper angle 

        # define the control mode server 
        try :
            control_mode_srv = rospy.Service(rospy.get_param('/srv_info/mode_srv_name'),
                                            CtrlModeSelect,
                                            control_mode_check)    
            rospy.loginfo("----- Ready to handle the control_mode_srv! ---- ")
        except rospy.ServiceException as serEx:
            rospy.logerr("----- NOT Ready to handle the control_mode_srv! ---- ")

        # subscribe just for the first init gripper and wrist angle change do not use in Auto and Teach Mode 
        #if G_CONTROL_MODE == rospy.get_param('/control_mode/landing_mode'):
        
        wrist_gripper_sub = rospy.Subscriber('/robot_arm_state',
                                            RobotState,
                                            wrist_gripper_callback,
                                            queue_size = 1)
        rospy.loginfo('----- Subscribed to /robot_arm_state----')
        rospy.loginfo("------Ready to handle ONLY the wrist_gripper_sub! -----")
        
        rospy.spin()

####### dual arm robot auto mode ##########
    def start_auto_mode(self):
        # read motion group teach data
        # motion group 2
        file_name = [rospy.get_param('/auto_mode/loading_path_default')]
        dual_arm_servo = g_dual_arm_nh.get_arm_servo_list()
        # 读取左右臂运动轨迹点数据
        traj_points_dual_arm, dual_arm_current_pose = dual_arm_manager.MotionGroup(dual_arm, 
                                point_num=POSE_NUM, 
                                time_t= 1/SAMPLING_FREQUENCY,
                                is_write_point=False,
                                is_read_point=True,
                                is_debug=False,
                                group_id=2).read_motion_point(
                                    file_name_list=file_name)
        g_dual_arm_nh.set_all_servo_mode(CONTROL_MODE="CIRCLE")
        # 再现
        for it in dual_arm_current_pose:
            # wrist and gripper control
            if it in dual_arm_current_pose[:3]:
                # trajectory control
                for SERVO_ID in dual_arm_servo:
                    g_dual_arm_nh.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=it[SERVO_ID],
                                                vel=VEL.get('Very_LOW_SPEED'), 
                                                iterval=2000,
                                                t_acc=50,
                                                t_dec=60, 
                                                power=9500, 
                                                mean_dps=40, 
                                                servo_id=SERVO_ID))
                # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
                g_wrist_gripper.set_servo_angle(servo_idx=0, angle=it[3])
                g_wrist_gripper.set_servo_angle(servo_idx=1, angle=it[8])

                # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
                g_wrist_gripper.set_servo_angle(servo_idx=2, angle=it[7])
                g_wrist_gripper.set_servo_angle(servo_idx=3, angle=it[9])
                time.sleep(MEDIA_TIME_DELAY)
            else:
                for SERVO_ID in dual_arm_servo:
                    g_dual_arm_nh.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=it[SERVO_ID], 
                                                vel=VEL.get('LOW_SPEED'), 
                                                iterval=2000, 
                                                t_acc=40,
                                                t_dec=40, 
                                                power=9500, 
                                                mean_dps=60, 
                                                servo_id=SERVO_ID))
                # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
                g_wrist_gripper.set_servo_angle(servo_idx=0, angle=it[3])
                g_wrist_gripper.set_servo_angle(servo_idx=1, angle=it[8])

                # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
                g_wrist_gripper.set_servo_angle(servo_idx=2, angle=it[7])
                g_wrist_gripper.set_servo_angle(servo_idx=3, angle=it[9])
                time.sleep(MEDIA_TIME_DELAY)               
    
    # start teach mode 
    def start_teach_mode(self):
        # read angle data and write these info into txt file.
        rospy.loginfo("THREADING-{}, start teach mode !".format(self.thread_name))
        time.sleep(2)
        
        g_dual_arm_nh.set_all_servo_mode(CONTROL_MODE="DAMPING")
        dual_arm_current_pose = MotionGroup(
                        g_dual_arm_nh, 
                        point_num=POSE_NUM, 
                        time_t= 1/self.sampling_frequency, 
                        is_write_point=True,
                        is_read_point=True, 
                        is_debug=False, 
                        group_id=rospy.get_param('/teach_mode/group_id')).write_motion_point()

    def start_servo_mode(self):
        # sub JointState and RobotState to control the servo motors 
        rospy.loginfo("---- THREADING {}, start servo mode !----".format(self.thread_name))
        # move to the zero position
        servo_list = g_dual_arm_nh.get_arm_servo_list()
        for SERVO_ID in servo_list:
            g_dual_arm_nh.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=DUAL_ARM_ZERO_ANGLE_POSE[SERVO_ID], 
                                vel=VEL.get('VERY_LOW_SPEED'),
                                iterval=2*controlPeriod,
                                t_acc=80,
                                t_dec=60, 
                                power=9000, 
                                mean_dps=20, 
                                servo_id=SERVO_ID))
            time.sleep(SHORT_TIME_DELAY)

        # set the robot arm wrist joint and gripper angle
        # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
        g_wrist_gripper.set_servo_angle(servo_idx=0, angle=DUAL_ARM_ZERO_ANGLE_POSE[3])
        g_wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_ZERO_ANGLE_POSE[8])

        # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
        g_wrist_gripper.set_servo_angle(servo_idx=2, angle=DUAL_ARM_ZERO_ANGLE_POSE[7])
        g_wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_ZERO_ANGLE_POSE[9])

        time.sleep(1)

        # subscribe the RobotState /robot_arm_state for servo mode control use only
        servo_mode_sub = rospy.Subscriber('/robot_arm_state',
                                        RobotState,
                                        servo_mode_callback,
                                        queue_size = 2)
        # spin() simply keeps python from exiting until this node is stopped
        rospy.spin()

    def start_landing_mode(self):
        # force landing pose for dual_arm robot
        rospy.loginfo("----THREADING-{}, start landing mode !".format(self.thread_name))
        servo_list = g_dual_arm_nh.get_arm_servo_list()
        for SERVO_ID in servo_list:
            g_dual_arm_nh.set_single_servo_pose(
                        Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID], 
                                vel=VEL.get('VERY_LOW_SPEED'),
                                iterval=2*controlPeriod,
                                t_acc=80,
                                t_dec=60, 
                                power=9000, 
                                mean_dps=20, 
                                servo_id=SERVO_ID))
            time.sleep(SHORT_TIME_DELAY)

        # set the robot arm wrist joint and gripper angle
        # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
        g_wrist_gripper.set_servo_angle(servo_idx=0, angle=DUAL_ARM_INIT_POSE[3])
        g_wrist_gripper.set_servo_angle(servo_idx=1, angle=DUAL_ARM_INIT_POSE[8])

        # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
        g_wrist_gripper.set_servo_angle(servo_idx=2, angle=DUAL_ARM_INIT_POSE[7])
        g_wrist_gripper.set_servo_angle(servo_idx=3, angle=DUAL_ARM_INIT_POSE[9])


##### function callback #####
def wrist_gripper_callback(RobotState):
    # dual_arm_nh = args[1]
    # wrist_gripper= args[2]
    
    # get the msg data, and exchange the unit from rad to deg using fast para Pi_Para
    left_wrist = RobotState.leftArmPosition[3]*Pi_Para
    left_gripper = RobotState.leftArmPosition[4]*Pi_Para

    right_wrist = RobotState.rightArmPosition[3]*Pi_Para
    right_gripper = RobotState.rightArmPosition[4]*Pi_Para

    # set the robot arm wrist joint and gripper angle
    # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
    g_wrist_gripper.set_servo_angle(servo_idx=0, angle=left_wrist)
    g_wrist_gripper.set_servo_angle(servo_idx=1, angle=left_gripper)

    # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
    g_wrist_gripper.set_servo_angle(servo_idx=2, angle=right_wrist)
    g_wrist_gripper.set_servo_angle(servo_idx=3, angle=right_gripper)
    rospy.loginfo("Wrist and Gripper control call back success! ")
    # time.sleep(SHORT_TIME_DELAY)


##### function callback #####
def control_mode_check(request):
    # callback function to change the  control mode flag
    if request.ctrl_mode_index == 0 :
        G_CONTROL_MODE = 0   # change into auto mode =0
        return CtrlModeSelectResponse(True)
    elif request.ctrl_mode_index == 1:
        G_CONTROL_MODE = 1  # change into teach mode =1
        return CtrlModeSelectResponse(True)
    elif request.ctrl_mode_index == 2:
        G_CONTROL_MODE = 2  # change into servo mode =2
        return CtrlModeSelectResponse(True)
    elif request.ctrl_mode_index == 3:
        G_CONTROL_MODE = 3   # change into landing  mode =3
        return CtrlModeSelectResponse(True)
    else:
        G_CONTROL_MODE = 3 # change into default
        rospy.logerr("REQUEST ERROR: Client call is error cmd! should be in {0,1,2,3} ")
        return CtrlModeSelectResponse(False)

##### function callback #####
def servo_mode_callback(RobotState):
    # # input check 
    # dual_arm_nh = args[1]
    # wrist_gripper = args[2]
    servo_list = g_dual_arm_nh.get_arm_servo_list()

    if (g_dual_arm_nh or g_wrist_gripper or servo_list) is None:
        rospy.logerr("ERROR from (dual_arm_nh or wrist_gripper or servo_list) is None!") 
        return False
    for SERVO_ID in servo_list[:3]:
    # 0,1,2 left
        if SERVO_ID == 1:
            # 1号舵机转动方向相反
            g_dual_arm_nh.set_single_servo_pose(
                Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID] - RobotState.leftArmPosition[SERVO_ID]*Pi_Para,
                                vel=VEL.get('LOW_SPEED'),
                                iterval=controlPeriod,
                                t_acc=80,
                                t_dec=60, 
                                power=10000, 
                                mean_dps=30, 
                                servo_id=SERVO_ID))
        else:
            # servo 0,2 转动方向相同
            g_dual_arm_nh.set_single_servo_pose(
                Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID] + RobotState.leftArmPosition[SERVO_ID]*Pi_Para,
                                    vel=VEL.get('LOW_SPEED'),
                                    iterval=controlPeriod,
                                    t_acc=80,
                                    t_dec=60, 
                                    power=10000, 
                                    mean_dps=30, 
                                    servo_id=SERVO_ID))
    
    for SERVO_ID in servo_list[4:7]:
    # 4,5,6 right arm
        g_dual_arm_nh.set_single_servo_pose(
        Pose=generate_pose_vtc(angle=DUAL_ARM_INIT_POSE[SERVO_ID] + RobotState.rightArmPosition[SERVO_ID]*Pi_Para, 
                                vel=VEL.get('LOW_SPEED'),
                                iterval=controlPeriod,
                                t_acc=80,
                                t_dec=60, 
                                power=10000, 
                                mean_dps=30, 
                                servo_id=SERVO_ID))
    # set the robot arm wrist joint and gripper angle
    # 左臂手腕手爪位置伺服，0（IO5）-》it-3，1(IO6)->it-[8]
    g_wrist_gripper.set_servo_angle(servo_idx=0, angle=RobotState.leftArmPosition[3]*Pi_Para)
    g_wrist_gripper.set_servo_angle(servo_idx=1, angle=RobotState.leftArmPosition[4]*Pi_Para)

    # 右臂手腕手爪位置伺服，2（IO12）-》it-7，3(IO13)->it-[9]
    g_wrist_gripper.set_servo_angle(servo_idx=2, angle=RobotState.rightArmPosition[3]*Pi_Para)
    g_wrist_gripper.set_servo_angle(servo_idx=3, angle=RobotState.rightArmPosition[4]*Pi_Para)
    time.sleep(SHORT_TIME_DELAY)


# 增加关节舵机角度的vel和iterval参数动态调整接口API
def generate_pose_vtc(angle, servo_id, vel=100, iterval=5000, t_acc=35, t_dec=35, power=10000, mean_dps=70.0):
    pose_tmp = {'angle': angle, 'is_mturn': False, 'interval': iterval,
                'vel': vel, 't_acc': t_acc,
                't_dec': t_dec, 'power': power,
                'mean_dps': mean_dps}
    gen_pose = Pose(POSE=pose_tmp, SERVO_ID=servo_id)
    return gen_pose


####### PING HOST IP THREADING #####
# ping_host_ip return HOST_STATE = False or True
def ping_host_ip(threadName, this_ip, host_ip):
    # 记录开始执行的时间
    start_time = time.time()
    retry_times = 0
    count = 2
    interval = 0.5
    size = 2
    HOST_STATE = False
    # 定义用来 ping host ip
    while True:
        if (this_ip and host_ip) != None: 
            # 构造ping命令
            command = 'ping -c %s' % count + " -i %s" % interval + " -w %s" % size + " %s" % host_ip
            try:
                # <subprocess.Popen object at 0x7f6f93d6df10>
                res = subprocess.Popen([command],stdin=subprocess.PIPE,
                                                 stdout=subprocess.PIPE,
                                                 stderr = subprocess.PIPE,shell = True) 

            except subprocess.CalledProcessError,err:
                rospy.loginfo("HOST_IP_CHECK: WARNING - The HOST IP {} " +
                                "is NOT reachable! retry_times {} !".format(host_ip,
                                                                         retry_times))
            if res != None:
                rospy.loginfo("HOST_IP_CHECK: The HOST_IP {} is reachable! "+
                                ",STATE INFO has updated! ".format(host_ip))
                # rospy.loginfo('HOST_IP_CHECK: Time used ：%s' % (time.time() - start_time))
                
                # 提取返回值
                out = res.stdout.read().decode('utf-8')
                # printing ping results
                rospy.loginfo("---Host ip Ping results PRINTING: {}".format(out))
                regex = r'time=(.+?)ms'
                ping_results = re.findall(regex, out)

                # ping 延迟结果排序
                ping_results.sort()
                #检查下标
                index = int(0.1*int(count))
                if len(ping_results) == 0:
                    print("HOST_IP_CHECK: Error!ping_results No data!")
                    HOST_STATE = False
                    break   # ping连通性，结束循环
                elif len(ping_results) < index :
                    print("HOST_IP_CHECK: Error! Index out of range!")
                    HOST_STATE = False
                    break    # ping连通性，结束循环
                else:
                     #change the host ip state into True
                    print(ping_results[index])
                    HOST_STATE = True  # ping连通性，True
                   
            else:   
                rospy.loginfo("HOST_IP_CHECK: WARNING - The HOST IP {} is NOT reachable! ".format(host_ip))
                rospy.loginfo("HOST_IP_CHECK: WARNING - HOST IP {} PING retry {} Times! ".format(host_ip, retry_times))
                retry_times = retry_times + 1
        else:
            rospy.logerr("HOST_IP_CHECK: ERROR - error form This or Host ip {} / {} ,PLEASE check Parameter Server IP INFO !".format(this_ip,host_ip))
            HOST_STATE = False  # the input host ip address error, check the host ip
            break
        time.sleep(10)  # 延迟10s, 循环

    return HOST_STATE          #   disconnected - change the host ip state flag into False



######### State PUB ##############
def statePub(pub_freq = 5):
    # 创建舵机关节信息发布者话题
    # 默认状态发布频率5hz
    # main function to construct new control threading
    rospy.loginfo("State info Pub Node init success!")

    # 临时变量
    angle_tmp = []
    power_tmp = []
    tempture_tmp = []

    # ros pub node init 
    # node name:   dual_arm_state_pub_node
    # node topic:  /servo_info
    # rospy.init_node(rospy.get_param('/node_info/state_pub_node_name'), 
    #         anonymous=True)
    try:
        servo_pub = rospy.Publisher(rospy.get_param('/node_info/state_pub_topic_name'),
                    TelemetryInfo,
                    queue_size = 2)
        rospy.loginfo("-----State Publsiher Topic init success!-----")
    except rospy.ROSException:
        rospy.logerr("-----State Publsiher Topic init ERROR !-----")
        return -1

    pub_rate = rospy.Rate(pub_freq)         # default pub frequence is 5 
    #### Create TelemetryInfo ####
    msg =TelemetryInfo()
    rospy.loginfo('-----Robot Telemetry Info Publishing-----')
    while not rospy.is_shutdown():
    # publish the servo angle and other info
        for SERVO in g_dual_arm_nh.get_arm_servo_list():
            angle_tmp.append(g_dual_arm_nh.read_single_servo_angle(SERVO_ID=SERVO))
            power_tmp.append(g_dual_arm_nh.get_servo_status(SERVO_ID=SERVO).get('power'))
            tempture_tmp.append(g_dual_arm_nh.get_servo_status(SERVO_ID=SERVO).get('temp'))

            rospy.loginfo(angle_tmp)
        #rospy.loginfo(angle_tmp,power_tmp, tempture_tmp)
        #print(angle_tmp,power_tmp, tempture_tmp)

        #### Create TelemetryInfo msg ####
        msg.header.stamp = rospy.Time.now()
        msg.leftArmJointAngle.append(angle_tmp[0])
        msg.leftArmJointAngle.append(angle_tmp[1])
        msg.leftArmJointAngle.append(angle_tmp[2])
        msg.leftArmJointAngle.append(0)
        msg.leftArmJointAngle.append(0)

        msg.rightArmJointAngle.append(angle_tmp[3])
        msg.rightArmJointAngle.append(angle_tmp[4])
        msg.rightArmJointAngle.append(angle_tmp[5])
        msg.rightArmJointAngle.append(0)
        msg.rightArmJointAngle.append(0)

        msg.leftArmJointPower.append(power_tmp[0])
        msg.leftArmJointPower.append(power_tmp[1])
        msg.leftArmJointPower.append(power_tmp[2])
        msg.leftArmJointPower.append(0)
        msg.leftArmJointPower.append(0)

        msg.rightArmJointPower.append(power_tmp[3])
        msg.rightArmJointPower.append(power_tmp[4])
        msg.rightArmJointPower.append(power_tmp[5])
        msg.rightArmJointPower.append(0)
        msg.rightArmJointPower.append(0)

        msg.leftArmJointTemp.append( tempture_tmp[0])
        msg.leftArmJointTemp.append( tempture_tmp[1])
        msg.leftArmJointTemp.append( tempture_tmp[2])
        msg.leftArmJointTemp.append( 0)
        msg.leftArmJointTemp.append( 0)

        msg.rightArmJointTemp.append( tempture_tmp[3])
        msg.rightArmJointTemp.append( tempture_tmp[4])
        msg.rightArmJointTemp.append( tempture_tmp[5])
        msg.rightArmJointTemp.append(0)
        msg.rightArmJointTemp.append(0)

    # 发布消息	
    servo_pub.publish(msg)    
    # clear msg buffer
    del msg.leftArmJointAngle[:]
    del msg.rightArmJointAngle[:]
    del msg.leftArmJointPower[:]
    del msg.rightArmJointPower[:]
    del msg.leftArmJointTemp[:]
    del msg.rightArmJointTemp[:]
    # 清空临时变量
    angle_tmp = []
    power_tmp = []
    tempture_tmp = []

    pub_rate.sleep()


######### MAIN ##############
rospy.loginfo("######### MAIN ##############!")

def main():

    # 创建新线程
    controlThreadName = "control"
    statePubThreadName = "pub"
    pingThreadName = "ping"
    
    # THREAD1 -》》》》》network moniter node 
    thread1 = dfrThread(threadID=1,
                        name=pingThreadName,
                        controlPeriod=None,
                        samplingFrequency=None)
    rospy.loginfo("-----Threading network ping STARTED!-----")

    try:
        rospy.init_node(rospy.get_param('/node_info/robot_control_node_name'),anonymous=True)
        rospy.loginfo("-----Dual Arm Control and State Publsiher Node init success!-----")
    except rospy.ROSInitException as rosInitEx:
        rospy.logerr("-----Dual Arm Control and State Publsiher Node init ERROR:{}".format(str(rosInitEx)))

    # THREAD2 -》》》》》 state publisher node 
    thread2 = dfrThread(threadID=2,
                        name=statePubThreadName,
                        controlPeriod=None,
                        samplingFrequency=SAMPLING_FREQUENCY)
    rospy.loginfo("-----Threading robot state publisher STARTED!-----")

    # THREAD3 -》》》》》 control node 
    thread3 = dfrThread(threadID=3,
                        name=controlThreadName,
                        controlPeriod=3000,
                        samplingFrequency=SAMPLING_FREQUENCY)

    rospy.loginfo("-----Threading dfr control STARTED!-----")

    # 开启新线程 ,执行线程任务
    thread1.start()
    thread2.start()
    thread3.start()

    # 等待线程完成
    thread1.join()
    thread2.join()
    thread3.join()

    rospy.loginfo("-----退出主线程，主控节点结束!-----")


# main function
if __name__ == '__main__':
    # 创建一个logger_debug
    import sys

    #logger = logging.getLogger()
    #logger.setLevel(logging.DEBUG)  # Log等级总开关-debug
    #print("Logging Set Info: logger.setLevel(logging.DEBUG") 
    try:
        main()    
    except KeyboardInterrupt:
        rospy.loginfo("-----KeyboardInterrupt: 主程序正在退出!-----")
